/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "pclicp.h"

#include <stdio.h>
#include <iostream>
#include "kdl/jntarray.hpp"
#include <qdebug.h>
#include <qfile.h>
#include "messagelog.h"

//using namespace cv;
using namespace std;

#define D_MAX_INTEGE 2147483647
#define D_MAX_CALCULATE_TIME_OUT_COUNT 4
#define D_MAX_CALCULATE_FAILED_COUNT 4


PclIcp::PclIcp()
{
    robotId=-1;
    isMatchSucess=0;

    calculationParemeter.method=E_PCL_ICP_METHOD_IterativeClosestPoint;
    calculationParemeter.maximumIterations=500;
    calculationParemeter.transformationEpsilon=2;
    calculationParemeter.euclideanFitnessEpsilon=0.2;


//    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);

    baseMap= (pcl::PointCloud<pcl::PointXYZRGB>::Ptr)(new pcl::PointCloud<pcl::PointXYZRGB>);
    baseCompareMap=(pcl::PointCloud<pcl::PointXYZRGB>::Ptr)(new pcl::PointCloud<pcl::PointXYZRGB>);
    laserOriginMap=(pcl::PointCloud<pcl::PointXYZRGB>::Ptr)(new pcl::PointCloud<pcl::PointXYZRGB>);
    localMap=(pcl::PointCloud<pcl::PointXYZRGB>::Ptr)(new pcl::PointCloud<pcl::PointXYZRGB>);
    localCompareMap=(pcl::PointCloud<pcl::PointXYZRGB>::Ptr)(new pcl::PointCloud<pcl::PointXYZRGB>);
    transformedMap=(pcl::PointCloud<pcl::PointXYZRGB>::Ptr)(new pcl::PointCloud<pcl::PointXYZRGB>);
//    viewer =(new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer=(boost::shared_ptr<pcl::visualization::PCLVisualizer>)(new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer->setBackgroundColor (0, 0, 0);
    viewer->addCoordinateSystem (10.0);
    viewer->initCameraParameters ();

    //---------初始化线程锁--------
    pthread_mutexattr_init(&matchAttr);
    pthread_mutexattr_setprotocol(&matchAttr,PTHREAD_PRIO_INHERIT);
    pthread_mutex_init(&matchMutex,&matchAttr);

    setMatchCalculateParameter(calculationParemeter);
    userEstimateVehicleInMapMatrix=Eigen::Matrix4f::Identity();
    userEstimateMapInVehicleMatrix=Eigen::Matrix4f::Identity();
    calculateTimeOutCount=0;
    calculateFailedCount=0;



}

int PclIcp::setVehiclePoseEstimation(double x, double y, double r)
{
    pthread_mutex_lock(&matchMutex);
//    pcl::TransformationEstimationPtr tmpEst;
//    pcl.setTransformationEstimation (const TransformationEstimationPtr &te)

    calculateEstimateInMapMatrix_last=Eigen::Matrix4f::Identity();
    calculateEstimateInMapMatrix_new=Eigen::Matrix4f::Identity();

    Eigen::AngleAxisd rollAngle(0.0, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(0.0, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(r, Eigen::Vector3d::UnitZ());

    Eigen::Quaterniond q = yawAngle*pitchAngle*rollAngle;
    Eigen::Matrix3d rotateMa = q.matrix();

    userEstimateVehicleInMapMatrix=Eigen::Matrix4f::Identity();
    userEstimateVehicleInMapMatrix(0,0)=rotateMa(0,0);
    userEstimateVehicleInMapMatrix(0,1)=rotateMa(0,1);
    userEstimateVehicleInMapMatrix(0,2)=rotateMa(0,2);
    userEstimateVehicleInMapMatrix(1,0)=rotateMa(1,0);
    userEstimateVehicleInMapMatrix(1,1)=rotateMa(1,1);
    userEstimateVehicleInMapMatrix(1,2)=rotateMa(1,2);
    userEstimateVehicleInMapMatrix(2,0)=rotateMa(2,0);
    userEstimateVehicleInMapMatrix(2,1)=rotateMa(2,1);
    userEstimateVehicleInMapMatrix(2,2)=rotateMa(2,2);

    //为什么要取反，左手坐标系，右手坐标系区别么？??
    userEstimateVehicleInMapMatrix(0,3)=x;
    userEstimateVehicleInMapMatrix(1,3)=y;
    userEstimateVehicleInMapMatrix(2,3)=0;

    userEstimateMapInVehicleMatrix=userEstimateVehicleInMapMatrix.inverse();
    //求逆矩阵为什么不对
//        userEstimateMapInVehicleMatrix(0,3)=-userEstimateVehicleInMapMatrix(0,3);
//        userEstimateMapInVehicleMatrix(1,3)=-userEstimateVehicleInMapMatrix(1,3);

    pthread_mutex_unlock(&matchMutex);

    return 1;
}


int PclIcp::setMatchCalculateParameter(PclCalculationParameter paraIn)
{
    pthread_mutex_lock(&matchMutex);
    calculationParemeter=paraIn;
    matchDebugInfo.method=paraIn.method;
    matchDebugInfo.maximumIterations=paraIn.maximumIterations;
    matchDebugInfo.transformationEpsilon=paraIn.transformationEpsilon;
    matchDebugInfo.euclideanFitnessEpsilon=paraIn.euclideanFitnessEpsilon;
    pthread_mutex_unlock(&matchMutex);
    return 1;

}

int PclIcp::getMatchDebugInfo(PclMatchDebugInfo &infoOut)
{
    infoOut=matchDebugInfo;
    return 1;
}

int PclIcp::showPclIcpDebugWindow(bool isShowBaseMap,bool isShowBaseCompareMap,
                                  bool isShowLocalMap,bool isShowLocalCompareMap)
{
    pthread_mutex_lock(&matchMutex);

    viewer->removeAllPointClouds();
    if(true==isShowBaseMap)
    {
        showBaseMap();
    }
    if(true==isShowBaseCompareMap)
    {
        showBaseCompareMap();
    }
    if(true==isShowLocalMap)
    {
        showLocalMap();
    }
    if(true==isShowLocalCompareMap)
    {
//        showLocalCampareMap();
        showTransformedMap();

    }

//    showMatchWindow();
     pthread_mutex_unlock(&matchMutex);
    return 1;
}

int PclIcp::updatePclIcpDebugWindow()
{
//    while (!viewer->wasStopped ())
//    {
         viewer->spinOnce (100);
////           boost::this_thread::sleep (boost::posix_time::microseconds (100000));
//    }
       return 1;
}

int PclIcp::setLaserToVehicleInfo(LaserScanDeviceInfo infoIn)
{
    pthread_mutex_lock(&matchMutex);
    laserScanDeviceInfo=infoIn;
    calculateLaserToVehicleMatrix();
    pthread_mutex_unlock(&matchMutex);

    return 1;
}


int PclIcp::loadBaseMap(QString mapPath,double mPerPixIn,double x_offset,double y_offset,double r_offset)
{

    pthread_mutex_lock(&matchMutex);
    QFile tmpfile(mapPath);
    tmpfile.open(QIODevice::ReadOnly);

    //read width height
    QString line = tmpfile.readLine();
    line = tmpfile.readLine();
    line = tmpfile.readLine();
    int width , height;
    if (true!=line.isNull())
    {
        QString firstStr=line.section(" ",0,0);
        qDebug()<<firstStr;
        QString secondStr=line.section(" ",1,1);
        qDebug()<<secondStr;
        width=firstStr.toInt();
        height=secondStr.toInt();
    }
    else
    {
        pthread_mutex_unlock(&matchMutex);
        return -1;
    }


    //read doc
    tmpfile.readLine();
    QVector<QVector<unsigned char> > imagineData;
    imagineData.resize(height);
    char tmpChar;
    for(int i=0;i<height;i++)
    {
        for(int j=0;j<width;j++)
        {
            tmpfile.read((char*)&tmpChar, sizeof(tmpChar));
            imagineData[i].append(tmpChar);
        }
    }
    tmpfile.close();


    //pcl initial
    int color;
    baseMap->clear();
    pcl::PointXYZRGB p ;
    for ( int i=0; i<height; i++ )
    {
        for ( int j=0; j<width; j++ )
        {
            color=imagineData[i][j];
            if(color<10)
            {

                p.x = j*mPerPixIn+x_offset;
                p.y = (height-i)*mPerPixIn+y_offset;
                p.z = 0;
                p.b = 250;
                p.g = 250;
                p.r = 0;
                //点云中添加一个点
                baseMap->points.push_back( p );
            }
        }
    }

    baseMap->is_dense = false;
    cout<<"basemap点云共有"<<baseMap->size()<<"个点."<<endl;
    pthread_mutex_unlock(&matchMutex);
    return 1;

}

int PclIcp::setBaseMapRegion_m(double originX, double originY, double width, double height)
{
    pthread_mutex_lock(&matchMutex);

    baseCompareMap->clear();
    int pointSize=baseMap->size();
    PointT p ;
    for ( int i=0; i<pointSize; i++ )
    {
        if(baseMap->points[i].x > originX && baseMap->points[i].x <originX+width
                &&baseMap->points[i].y > originY && baseMap->points[i].y <originX+height
                )
        {
            p.x = baseMap->points[i].x;
            p.y = baseMap->points[i].y;
            p.z = 0;
            p.b = 100;
            p.g = 250;
            p.r = 0;
            //点云中添加一个点
            baseCompareMap->points.push_back( p );
        }


    }

    baseCompareMap->is_dense = false;
    cout<<"baseCompareMap点云共有"<<baseCompareMap->size()<<"个点."<<endl;

    pthread_mutex_unlock(&matchMutex);
    return 1;
}



int PclIcp::calculateMatch_generalized()
{

}

//int PclIcp::calculateMatch_interative()
//{

////    最大迭代次数：Number of iterations has reached the maximum user imposed number of iterations
//    //(via setMaximumIterations)
////    两次变化矩阵之间的差值：The epsilon (difference) between the previous transformation and the current estimated transformation is smaller than an user imposed value
//           // (via setTransformationEpsilon)
////    均方误差（MSE）：The sum of Euclidean squared errors is smaller than a user defined threshold
//          //  (via setEuclideanFitnessEpsilon)


//   //creates an instance of an IterativeClosestPoint and gives it some useful information

//    pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> tmpIcp;

//   tmpIcp.setMaximumIterations(calculationParemeter.maximumIterations);
//   tmpIcp.setTransformationEpsilon(calculationParemeter.transformationEpsilon);
//   tmpIcp.setEuclideanFitnessEpsilon(calculationParemeter.euclideanFitnessEpsilon);
////   icp.setMaxCorrespondenceDistance(calculationParemeter.euclideanFitnessEpsilon);
//   tmpIcp.setInputCloud(localCompareMap);
//   tmpIcp.setInputTarget(baseCompareMap);

//   //Creates a pcl::PointCloud<pcl::PointXYZ> to which the IterativeClosestPoint can save the resultant cloud after applying the algorithm
////   pcl::PointCloud<pcl::PointXYZRGB> finalCloud;

//   //Call the registration algorithm which estimates the transformation and returns the transformed source (input) as output.
//   tmpIcp.align(*transformedMap);

//   //Return the state of convergence after the last align run.
//   //If the two PointClouds align correctly then icp.hasConverged() = 1 (true).
//   int tmSucess=tmpIcp.hasConverged();
//   std::cout << "has converged: " <<tmSucess  <<std::endl;
//   if(1!=tmSucess)
//   {
//      return -1;
//   }

//   //Obtain the Euclidean fitness score (e.g., sum of squared distances from the source to the target)
//   matchScore=tmpIcp.getFitnessScore();
////   maxMatchError=tmpIcp.getMaxCorrespondenceDistance();
//   std::cout << "score: " <<matchScore << std::endl;
//   std::cout << "maxMatchError: " <<maxMatchError << std::endl;
//   std::cout << "----------------------------------------------------------"<< std::endl;

//   //Get the final transformation matrix estimated by the registration method.
//   transformedMatrix=tmpIcp.getFinalTransformation();
//   std::cout << transformedMatrix << std::endl;
//   local_matchX=transformedMatrix(0,3);
//   local_matchY=transformedMatrix(1,3);
//   //旋转矩阵 --> 欧拉角
//   Eigen::Matrix3d tmp3D;
//   tmp3D(0,0)=transformedMatrix(0,0);
//   tmp3D(0,1)=transformedMatrix(0,1);
//   tmp3D(0,2)=transformedMatrix(0,2);
//   tmp3D(1,0)=transformedMatrix(1,0);
//   tmp3D(1,1)=transformedMatrix(1,1);
//   tmp3D(1,2)=transformedMatrix(1,2);
//   tmp3D(2,0)=transformedMatrix(2,0);
//   tmp3D(2,1)=transformedMatrix(2,1);
//   tmp3D(2,2)=transformedMatrix(2,2);
//   Eigen::Vector3d euler_angles = tmp3D.eulerAngles(2,1,0);
//   local_matchR=euler_angles(0);
//   cout << "yaw pitch roll = \n" << euler_angles.transpose()<<endl;

//   return 1;


////   Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());
////   Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());
////   Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());

////   Eigen::Quaternion<double> q = rollAngle * yawAngle * pitchAngle;

////   Eigen::Matrix3d rotationMatrix = q.matrix()
//}


int PclIcp::calculateMatch_interative()
{

//    最大迭代次数：Number of iterations has reached the maximum user imposed number of iterations
    //(via setMaximumIterations)
//    两次变化矩阵之间的差值：The epsilon (difference) between the previous transformation and the current estimated transformation is smaller than an user imposed value
           // (via setTransformationEpsilon)
//    均方误差（MSE）：The sum of Euclidean squared errors is smaller than a user defined threshold
          //  (via setEuclideanFitnessEpsilon)


   //creates an instance of an IterativeClosestPoint and gives it some useful information

   icp.setMaximumIterations(calculationParemeter.maximumIterations);
   icp.setTransformationEpsilon(calculationParemeter.transformationEpsilon);//设置０．００００１
   icp.setEuclideanFitnessEpsilon(calculationParemeter.euclideanFitnessEpsilon);//设置０
   icp.setMaxCorrespondenceDistance(calculationParemeter.maxCorrespondenceDistance);//初始距离偏差太大的点过滤掉
   icp.setInputCloud(localCompareMap);
   icp.setInputTarget(baseCompareMap);

   //Creates a pcl::PointCloud<pcl::PointXYZ> to which the IterativeClosestPoint can save the resultant cloud after applying the algorithm
//   pcl::PointCloud<pcl::PointXYZRGB> finalCloud;

   //Call the registration algorithm which estimates the transformation and returns the transformed source (input) as output.
   icp.align(*transformedMap);

   //Return the state of convergence after the last align run.
   //If the two PointClouds align correctly then icp.hasConverged() = 1 (true).
   int tmSucess=icp.hasConverged();
   std::cout << "has converged: " <<tmSucess  <<std::endl;
   if(1!=tmSucess)
   {
      return -1;
   }

   //Obtain the Euclidean fitness score (e.g., sum of squared distances from the source to the target)
   matchScore=icp.getFitnessScore();
//   maxMatchError=icp.getMaxCorrespondenceDistance();
   std::cout << "score: " <<matchScore << std::endl;
   std::cout << "maxMatchError: " <<maxMatchError << std::endl;
   std::cout << "----------------------------------------------------------"<< std::endl;

   //Get the final transformation matrix estimated by the registration method.
   calculateEstimateInMapMatrix_new=icp.getFinalTransformation();
   std::cout << calculateEstimateInMapMatrix_new << std::endl;


   calculateEstimateInMapMatrix_last=calculateEstimateInMapMatrix_new*calculateEstimateInMapMatrix_last;
   return 1;


//   Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());
//   Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());
//   Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());

//   Eigen::Quaternion<double> q = rollAngle * yawAngle * pitchAngle;

//   Eigen::Matrix3d rotationMatrix = q.matrix()
}

int PclIcp::calculateMatch_normals()
{

}

int PclIcp::calculateMatch_nonLinear()
{

}

int PclIcp::calculateMatch_joint()
{

}

int PclIcp::loadLocalMap(std::vector<double> pointX, std::vector<double> pointY)
{
    pthread_mutex_lock(&matchMutex);

    laserOriginMap->clear();

    PointT p ;
    for ( int i=0; i<pointX.size(); i++ )
    {
        p.x = pointX[i];
        p.y = pointY[i];
        p.z = 0;
        p.b = 240;
        p.g = 40;
        p.r = 250;
        //点云中添加一个点
        laserOriginMap->points.push_back( p );

    }

    laserOriginMap->is_dense = false;
    cout<<"laserOriginMap点云共有"<<laserOriginMap->size()<<"个点."<<endl;

//    Eigen::Matrix4f tmpMatrix=Eigen::Matrix4f::Identity();
//    pcl::transformPointCloud(*laserOriginMap, *localMap, tmpMatrix);
      Eigen::Matrix4f tmpMatrix=calculateEstimateInMapMatrix_last*userEstimateVehicleInMapMatrix*laserToVehicleMatrix;
//    Eigen::Matrix4f tmpMatrix=laserToVehicleMatrix*userEstimateMapInVehicleMatrix;
//    Eigen::Matrix4f tmpMatrix=userEstimateMapInVehicleMatrix*laserToVehicleMatrix;
      pcl::transformPointCloud(*laserOriginMap, *localMap, tmpMatrix);
//    pcl::transformPointCloud(*laserOriginMap, *localMap, laserToVehicleMatrix);

    pthread_mutex_unlock(&matchMutex);


    return 1;
}



int PclIcp::setLocalMapRegion_m(double originX, double originY, double width, double height)
{
    pthread_mutex_lock(&matchMutex);

    localCompareMap->clear();
    int pointSize=localMap->size();
    PointT p ;
    for ( int i=0; i<pointSize; i++ )
    {
        if(localMap->points[i].x > originX && localMap->points[i].x <originX+width
                &&localMap->points[i].y > originY && localMap->points[i].y <originX+height
                )
        {
            p.x = localMap->points[i].x;
            p.y = localMap->points[i].y;
            p.z = 0;
            p.b = 100;
            p.g = 250;
            p.r = 100;
            //点云中添加一个点
           localCompareMap->points.push_back( p );
        }


    }

   localCompareMap->is_dense = false;
   cout<<"localCompareMap点云共有"<<localCompareMap->size()<<"个点."<<endl;

    pthread_mutex_unlock(&matchMutex);
    return 1;
}

int PclIcp::calculateMatch()
{

    //for debug
//    matchDebugInfo.baseCompareCols=baseCompareMap->cols;
//    matchDebugInfo.baseCompareRows=baseCompareMap->rows;
//    matchDebugInfo.localCompareCols=localCompareMap->cols;
//    matchDebugInfo.localCompareRows=localCompareMap->rows;

    struct timespec startTime,endTime;
    clock_gettime(CLOCK_MONOTONIC, &startTime);

    int tmpReturn;
    switch(calculationParemeter.method)
    {
    case E_PCL_ICP_METHOD_GeneralizedIterativeClosestPoint:
    {
        tmpReturn= calculateMatch_generalized();
        break;
    }
    case E_PCL_ICP_METHOD_IterativeClosestPoint:
    {
        tmpReturn= calculateMatch_interative();
        break;
    }
    case E_PCL_ICP_METHOD_IterativeClosestPointWithNormals:
    {
        tmpReturn= calculateMatch_normals();
        break;
    }
    case E_PCL_ICP_METHOD_IterativeClosestPointNonLinear:
    {
        tmpReturn= calculateMatch_nonLinear();
        break;
    }
    case E_PCL_ICP_METHOD_JointIterativeClosestPoint:
    {
        tmpReturn= calculateMatch_joint();
        break;
    }

    default:
    {
        tmpReturn=-1;
        qDebug()<<"unknow method =="<<calculationParemeter.method;
    }
    }
    if(1==tmpReturn)
    {
        isMatchSucess=true;
        matchDebugInfo.isMatchSucess=isMatchSucess;
        calculateFailedCount=0;
    }
    else
    {
        isMatchSucess=false;
        matchDebugInfo.isMatchSucess=isMatchSucess;
        calculateFailedCount++;
    }
    clock_gettime(CLOCK_MONOTONIC, &endTime);

    calculateConsumeTime=1000000000*(endTime.tv_sec-startTime.tv_sec)+endTime.tv_nsec-startTime.tv_nsec;
    calculateConsumeTime=calculateConsumeTime/1000.0;

    //失败次数判断
    if(calculateFailedCount>D_MAX_CALCULATE_FAILED_COUNT)
    {
        addMsg(ENUM_MSG_ERROR, "RosApi", "PclIcp", 8201, -1 );
    }

    //求解超时判断
    if(calculateConsumeTime>calculationParemeter.maxCalculateTime*1000000.0)
    {
        calculateTimeOutCount++;
    }
    else
    {
        calculateTimeOutCount=0;
    }
    if(calculateTimeOutCount>D_MAX_CALCULATE_TIME_OUT_COUNT)
    {
        addMsg(ENUM_MSG_ERROR, "RosApi", "PclIcp", 8200, -1 );
    }

    //for debug
    matchDebugInfo.pos_x=vehicle_match_x;
    matchDebugInfo.pos_y=vehicle_match_y;
    matchDebugInfo.pos_r=vehicle_match_r;
    matchDebugInfo.matchScore=matchScore;


    return tmpReturn;
}

int PclIcp::calculateMatch(double &xOut, double &yOut, double &rOut, int &isCalculateSucess_out,
                           double &maxMatchErrorOut, double &matchScoreOut)
{
    pthread_mutex_lock(&matchMutex);
    int tmpReturn;
    tmpReturn= calculateMatch();
    matchScoreOut=matchScore;
    maxMatchErrorOut=maxMatchError;
    matchDebugInfo.consumeTimeOut=calculateConsumeTime;

    getVehiclePosition_m(xOut,yOut,rOut,isCalculateSucess_out);
    pthread_mutex_unlock(&matchMutex);
    return tmpReturn;
}





//int PclIcp::imagine_threshold_process(Mat *imagine)
//{
////    int threshold_value = 1;//pgm: 0x80表示未知区域，0x00表示障碍物，0xfe表示已知区域
////    int threshold_type = 0;;
////    int const max_BINARY_value = 255;

////    /// 将图片转换成灰度图片
//////     cvtColor( *imagine, *imagine, CV_RGB2GRAY );


////      /* 0: 二进制阈值
////         1: 反二进制阈值
////         2: 截断阈值
////         3: 0阈值
////         4: 反0阈值
////       */

////     threshold( *imagine, *imagine, threshold_value, max_BINARY_value,threshold_type );


//}



//int PclIcp::getLaserPosition_m(double &xOut, double &yOut, double &rOut, int &isCalculateSucess_out)
//{
//    xOut=local_matchX;
//    yOut=local_matchY;
//    rOut=local_matchR;
//    isCalculateSucess_out=isMatchSucess;
//    return 1;

//}

int PclIcp::getVehiclePosition_m(double &xOut, double &yOut, double &rOut, int &isCalculateSucess_out)
{
    if(false==isMatchSucess)
    {
        return -1;
    }
//    Eigen::Matrix4f tmpLastMat=calculateEstimateInMapMatrix_last.inverse();
////    //求逆矩阵为什么不对
////        tmpLastMat(0,3)=-estimateToActualMatrix_last(0,3);
////        tmpLastMat(1,3)=-estimateToActualMatrix_last(1,3);
//    Eigen::Matrix4f tmpNewMat=calculateEstimateInMapMatrix_new.inverse();
//    //求逆矩阵为什么不对
////        tmpNewMat(0,3)=-estimateToActualMatrix_new(0,3);
////        tmpNewMat(1,3)=-estimateToActualMatrix_new(1,3);
    vehicleActualMatrix=calculateEstimateInMapMatrix_last*userEstimateVehicleInMapMatrix;
    vehicle_match_x=vehicleActualMatrix(0,3);
    vehicle_match_y=vehicleActualMatrix(1,3);
    //旋转矩阵 --> 欧拉角１
//    Eigen::Matrix3d tmp3D;
//    tmp3D(0,0)=vehicleActualMatrix(0,0);
//    tmp3D(0,1)=vehicleActualMatrix(0,1);
//    tmp3D(0,2)=vehicleActualMatrix(0,2);
//    tmp3D(1,0)=vehicleActualMatrix(1,0);
//    tmp3D(1,1)=vehicleActualMatrix(1,1);
//    tmp3D(1,2)=vehicleActualMatrix(1,2);
//    tmp3D(2,0)=vehicleActualMatrix(2,0);
//    tmp3D(2,1)=vehicleActualMatrix(2,1);
//    tmp3D(2,2)=vehicleActualMatrix(2,2);
//    Eigen::Vector3d euler_angles = tmp3D.eulerAngles(2,1,0);
//    vehicle_match_r=euler_angles(0);//返回的区间（０，ｐｉ),不正常

    //旋转矩阵 --> 欧拉角２
    double roll,  pitch, yaw;
    //initial frame
    KDL::Rotation tmpRotation;
    tmpRotation(0,0)=vehicleActualMatrix(0,0);
    tmpRotation(0,1)=vehicleActualMatrix(0,1);
    tmpRotation(0,2)=vehicleActualMatrix(0,2);
    tmpRotation(1,0)=vehicleActualMatrix(1,0);
    tmpRotation(1,1)=vehicleActualMatrix(1,1);
    tmpRotation(1,2)=vehicleActualMatrix(1,2);
    tmpRotation(2,0)=vehicleActualMatrix(2,0);
    tmpRotation(2,1)=vehicleActualMatrix(2,1);
    tmpRotation(2,2)=vehicleActualMatrix(2,2);
    tmpRotation.GetRPY(roll,pitch,yaw);
    vehicle_match_r=yaw;

    xOut=vehicle_match_x;
    yOut=vehicle_match_y;
    rOut=vehicle_match_r;
    isCalculateSucess_out=isMatchSucess;

    return 1;

}

int PclIcp::showBaseMap()
{
//    imshow( "baseMap", *baseMap );
      pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> single_color(baseMap);
//      pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color (baseMap, 0, 255, 0);
      viewer->addPointCloud<pcl::PointXYZRGB> (baseMap,single_color, "baseMap");
      viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "baseMap");

      return 1;

}

int PclIcp::showBaseCompareMap()
{
//    imshow( "baseCompareMap", *baseCompareMap );

          pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> single_color(baseCompareMap);
    //      pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color (baseCompareMap, 0, 255, 0);
          viewer->addPointCloud<pcl::PointXYZRGB> (baseCompareMap,single_color, "baseCompareMap");
          viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "baseCompareMap");

          return 1;
}

int PclIcp::showLocalMap()
{
//    imshow( "localMap", *localMap );
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> single_color(localMap);
//      pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color (localMap, 0, 255, 0);
    viewer->addPointCloud<pcl::PointXYZRGB> (localMap,single_color, "localMap");
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "localMap");

    return 1;
}

int PclIcp::showLocalCampareMap()
{
//    imshow( "localCompareMap", *localCompareMap );
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> single_color(localCompareMap);
//      pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color (localCompareMap, 0, 255, 0);
    viewer->addPointCloud<pcl::PointXYZRGB> (localCompareMap,single_color, "localCompareMap");
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "localCompareMap");
    return 1;
}

int PclIcp::showTransformedMap()
{
//    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();

//	// Define a translation of 2.5 meters on the x axis.
//	transform_2.translation() << 2.5, 0.0, 0.0;

//	// The same rotation matrix as before; theta radians arround Z axis
//	transform_2.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));

//	// Print the transformation
//	printf("\nMethod #2: using an Affine3f\n");
//	std::cout << transform_2.matrix() << std::endl;

//	// Executing the transformation
//	pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>());
//	// You can either apply transform_1 or transform_2; they are the same
//	pcl::transformPointCloud(*source_cloud, *transformed_cloud, transform_2);


//    transformedMap->clear();
//    Eigen::Matrix4f tmpInverseMatrix=transformedMatrix.inverse();
//    pcl::transformPointCloud(*localCompareMap, *transformedMap,tmpInverseMatrix );
//    cout<<"transformedMap点云共有"<<transformedMap->size()<<"个点."<<endl;

    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> single_color(transformedMap);
    viewer->addPointCloud<pcl::PointXYZRGB> (transformedMap,single_color, "transformedMap");
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "transformedMap");

}

int PclIcp::showMatchWindow()
{
//    int thickness = -1;
//    int lineType = 8;

//    Point origin;
//    origin.x = 0;
//    origin.y = 0;

//    circle( *baseCompareMap,
//             origin,
//             10,
//             Scalar( 0, 0, 255 ),
//             thickness,
//             lineType );

//    putText( *baseCompareMap, "origin", origin, 0,1, Scalar( 100, 100, 0 ));


//     Point tmpX;
//    tmpX.x = 50;
//    tmpX.y = 0;

//    circle( *baseCompareMap,
//             tmpX,
//             10,
//             Scalar( 255, 0, 0 ),
//             thickness,
//             lineType );

//    putText( *baseCompareMap, "x=50", tmpX, 0,1, Scalar( 100, 100, 0 ));
//    line( *baseCompareMap,
//            origin,
//            tmpX,
//            Scalar( 0, 100, 0 ),
//            2,
//            lineType );

//    Point tmpY;
//    tmpY.x = 0;
//    tmpY.y = 50;

//    circle( *baseCompareMap,
//             tmpY,
//             10,
//             Scalar( 0, 255, 0 ),
//             thickness,
//             lineType );

//    putText( *baseCompareMap, "y=50", tmpY, 0,1, Scalar( 100, 100, 0 ));
//    line( *baseCompareMap,
//            origin,
//            tmpY,
//            Scalar( 0, 100, 0 ),
//            2,
//            lineType );


//    circle( *baseCompareMap,
//             Point( local_matchX, local_matchY),
//             10,
//             Scalar( 0, 0, 255 ),
//             thickness,
//             lineType );
//    putText( *baseCompareMap, "matchPoint", Point( local_matchX, local_matchY), 0,1, Scalar( 100, 100, 0 ));

//    thickness = 2;
//     line( *baseCompareMap,
//             Point( local_matchX, local_matchY),
//             Point( local_matchX+50*cos(local_matchR), local_matchY+50*sin(local_matchR)),
//             Scalar( 0, 100, 0 ),
//             thickness,
//             lineType );
//    imshow( "baseCompareMap", *baseCompareMap );
    return 1;
}

int PclIcp::calculateLaserToVehicleMatrix()
{

//    Eigen::Affine3f tmpAff= Eigen::Affine3f::Identity();
//    tmpAff.translation()<<laserScanDeviceInfo.pose_x,laserScanDeviceInfo.pose_y,
//            laserScanDeviceInfo.pose_z;
//    tmpAff.rotate(Eigen::AngleAxisf(laserScanDeviceInfo.pose_yaw, Eigen::Vector3f::UnitZ());



    Eigen::AngleAxisd rollAngle(laserScanDeviceInfo.pose_roll, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(laserScanDeviceInfo.pose_pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(laserScanDeviceInfo.pose_yaw, Eigen::Vector3d::UnitZ());

    Eigen::Quaterniond q = yawAngle*pitchAngle*rollAngle;
    Eigen::Matrix3d rotateMa = q.matrix();

    Eigen::Matrix4f tmpMatrix=Eigen::Matrix4f::Identity();
//    Eigen::Translation3f init_translation (laserScanDeviceInfo.pose_x,laserScanDeviceInfo.pose_y,
//                                           laserScanDeviceInfo.pose_z);
//    tmpMatrix = (init_translation * yawAngle*pitchAngle*rollAngle).matrix ();

    tmpMatrix(0,0)=rotateMa(0,0);
    tmpMatrix(0,1)=rotateMa(0,1);
    tmpMatrix(0,2)=rotateMa(0,2);
    tmpMatrix(1,0)=rotateMa(1,0);
    tmpMatrix(1,1)=rotateMa(1,1);
    tmpMatrix(1,2)=rotateMa(1,2);
    tmpMatrix(2,0)=rotateMa(2,0);
    tmpMatrix(2,1)=rotateMa(2,1);
    tmpMatrix(2,2)=rotateMa(2,2);

    tmpMatrix(0,3)=laserScanDeviceInfo.pose_x;
    tmpMatrix(1,3)=laserScanDeviceInfo.pose_y;
    tmpMatrix(2,3)=laserScanDeviceInfo.pose_z;

    laserToVehicleMatrix=tmpMatrix;//对的　华磊
//    laserToVehicleMatrix=tmpMatrix.inverse();//求逆矩阵为什么不对
//    laserToVehicleMatrix(0,3)=-tmpMatrix(0,3);
//    laserToVehicleMatrix(1,3)=-tmpMatrix(1,3);

    return 1;

}


int PclIcp::pgmToPointCould_cv(QString imgFilePath)
{
//    //Step 1
//    vector<cv::Mat> colorImgs, depthImgs;
//    // 彩色图和深度图
//    vector<Eigen::Isometry3d> poses;
//    // 相机位姿
//    ifstream fin("./pose.txt");
//    for ( int i=0; i<5; i++ )
//    {
//        boost::format fmt( "./%s/%d.%s" );
//        //图像文件格式
//        colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));
//        depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 ));
//        // 使用-1读取原始图像 double data[7] = {0};
//        for ( auto& d:data )
//            fin>>d;
//        Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );
//        Eigen::Isometry3d T(q);
//        T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] )); poses.push_back( T );
//    }
//    //Step 2...
//    // 相机内参
//    double cx = 325.5;
//    double cy = 253.5;
//    double fx = 518.0;
//    double fy = 519.0;
//    double depthScale = 1000.0;

//    //Step 3
//    // 定义点云使用的格式：这里用的是XYZRGB
//    typedef pcl::PointXYZRGB PointT;
//    typedef pcl::PointCloud<PointT> PointCloud;
//    // 新建一个点云
//    PointCloud::Ptr pointCloud( new PointCloud );
//    for ( int i=0; i<5; i++ )
//    {

//        cout<<"转换图像中: "<<i+1<<endl;
//        cv::Mat color = colorImgs[i];
//        cv::Mat depth = depthImgs[i];
//        Eigen::Isometry3d T = poses[i];
//        for ( int v=0; v<color.rows; v++ )
//        {
//            for ( int u=0; u<color.cols; u++ )
//            {
//                unsigned int d = depth.ptr<unsigned short> ( v )[u];
//                // 深度值 if ( d==0 ) continue;
//                // 为0表示没有测量到 Eigen::Vector3d point;
//                //point用来保存该点在相机坐标系下的坐标
//                point[2] = double(d)/depthScale;
//                //深度值根据深度相机规定的尺度因子确定
//                point[0] = (u-cx)*point[2]/fx;
//                //计算x坐标，该等式来源于针孔相机模型
//                point[1] = (v-cy)*point[2]/fy;
//                //计算y坐标，该等式也来源于针孔相机模型
//                Eigen::Vector3d pointWorld = T*point;
//                //将相机坐标系下的坐标变换到世界坐标系
//                PointT p ;
//                p.x = pointWorld[0]; p.y = pointWorld[1];
//                p.z = pointWorld[2]; p.b = color.data[ v*color.step+u*color.channels() ];
//                p.g = color.data[ v*color.step+u*color.channels()+1 ];
//                p.r = color.data[ v*color.step+u*color.channels()+2 ];
//                pointCloud->points.push_back( p );
//                //点云中添加一个点
//            }
//        }
//    }
//    pointCloud->is_dense = false;
//    cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
//    pcl::io::savePCDFileBinary("map.pcd", *pointCloud );

    return 1;

}

int PclIcp::pgmToPointCould(QString imgFilePath)
{

}

int PclIcp::pclTest()
{



//    pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget > Class Template Reference

//    pcl::IterativeClosestPoint< PointSource, PointTarget, Scalar > Class Template Reference

//    pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar > Class Template Reference

//    pcl::IterativeClosestPointNonLinear< PointSource, PointTarget, Scalar > Class Template Reference

//    pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar > Class Template Reference

//    pcl::registration::IncrementalICP< PointT, Scalar > Class Template Reference

//　　IterativeClosestPoint类提供了标准ICP算法的实现（The transformation is estimated based on SVD），算法迭代结束条件有如下几个:

//    最大迭代次数：Number of iterations has reached the maximum user imposed number of iterations (via setMaximumIterations)
//    两次变化矩阵之间的差值：The epsilon (difference) between the previous transformation and the current estimated transformation is smaller than an user imposed value (via setTransformationEpsilon)
//    均方误差（MSE）：The sum of Euclidean squared errors is smaller than a user defined threshold (via setEuclideanFitnessEpsilon)



    //Creates two pcl::PointCloud<pcl::PointXYZ> boost shared pointers and initializes them
       pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
       pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);

       // Fill in the CloudIn data
       cloud_in->width    = 5;
       cloud_in->height   = 1;
       cloud_in->is_dense = false;
       cloud_in->points.resize (cloud_in->width * cloud_in->height);
       for (size_t i = 0; i < cloud_in->points.size (); ++i)
       {
           cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
           cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
           cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
       }


       *cloud_out = *cloud_in;

       //performs a simple rigid transform on the point cloud
       for (size_t i = 0; i < cloud_in->points.size (); ++i)
           cloud_out->points[i].x = cloud_in->points[i].x + 1.5f;

       //creates an instance of an IterativeClosestPoint and gives it some useful information
       pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
       icp.setInputCloud(cloud_in);
       icp.setInputTarget(cloud_out);

       //Creates a pcl::PointCloud<pcl::PointXYZ> to which the IterativeClosestPoint can save the resultant cloud after applying the algorithm
       pcl::PointCloud<pcl::PointXYZ> Final;

       //Call the registration algorithm which estimates the transformation and returns the transformed source (input) as output.
       icp.align(Final);

       //Return the state of convergence after the last align run.
       //If the two PointClouds align correctly then icp.hasConverged() = 1 (true).
       std::cout << "has converged: " << icp.hasConverged() <<std::endl;

       //Obtain the Euclidean fitness score (e.g., sum of squared distances from the source to the target)
       std::cout << "score: " <<icp.getFitnessScore() << std::endl;
       std::cout << "----------------------------------------------------------"<< std::endl;

       //Get the final transformation matrix estimated by the registration method.
       std::cout << icp.getFinalTransformation() << std::endl;

       return 1;
}

void PclIcp::addMsg(int messageLevel, string componentName, string messageType,
                           int messageCode, int robotIdIn,
                           int parameter1, int parameter2, int parameter3, int parameter4,QString message)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotIdIn;
    tmpMsg.messageCode = messageCode;
   // tmpMsg.time = t1.tv_sec;


    QString infomationStr;

    switch(messageCode)
    {
    case 8201:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("icp算法连续求解失败错误！");
        break;
    }

    case 8200:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("icp算法连续超时错误！");
        break;
    }




    }


    tmpMsg.MessageInformation = infomationStr.toStdString();

    // qDebug()<<"53444444444444444444444"<< tmpMsg.messageCode;
    MessageLog::getInstance()->addMessage(tmpMsg);
}




